//#############################################################################

#ifndef SPEED_CALC_H
#define SPEED_CALC_H

//! \file   libraries\observers\speedcalc\include\speedcalc.h
//! \brief  Contains the public interface to the
//!         speed calculation
//!

//*****************************************************************************
//
// If building with a C++ compiler, make all of the definitions in this header
// have a C binding.
//
//*****************************************************************************
#ifdef __cplusplus
extern "C"
{
#endif

//*****************************************************************************
//
//! \defgroup SPEED_CALC SPEED_CALC
//! @{
//
//*****************************************************************************

// the includes
#ifdef __TMS320C28XX_CLA__
#include "libraries/math/src/float/CLAmath.h"
#else
#include <math.h>
#endif

#include "arm_math.h"
#include "math_constant.h"
#include "traj.h"

typedef struct _SPDCALC_obj_
{
    float32_t  ref;             // Input: reference set-point
    float32_t  fbk;             // Input: feedback
    float32_t  err;             // Error
    float32_t  out;             // Output: controller output
    float32_t  Kp;              // Parameter: proportional loop gain
    float32_t  Ki;              // Parameter: integral gain
    float32_t  Umax;            // Parameter: upper saturation limit
    float32_t  Umin;            // Parameter: lower saturation limit
    float32_t  Up;              // Data: proportional term
    float32_t  Ui;              // Data: integral term
    float32_t  speed_Hz;        // Output freq
    float32_t  thetaDelta;      // Parameter: theta maximum
} SPDCALC_Obj;

//! \brief Defines the ESMO handle
//!
typedef struct _SPDCALC_obj_ *SPDCALC_Handle;

// ***************************************
// extern functions
// ***************************************
//! \brief     Set the SPDCALC controller
//! \param[in] handle   The SPDCALC controller handle
SPDCALC_Handle SPDCALC_init(void *pMemory, const size_t numBytes);

//! \brief     Set the SPDCALC controller
//! \param[in] handle   The ESMO controller handle
void SPDCALC_reset(SPDCALC_Handle handle);

//! \brief     Set the SPDCALC controller
//! \param[in] handle      The SPDCALC controller handle
void SPDCALC_setParams(SPDCALC_Handle handle);

//! \brief     Set the SPDCALC controller
//! \param[in] handle      The SPDCALC controller handle
static inline float32_t SPDCALC_getSpeedHz(SPDCALC_Handle handle)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;

    return(obj->speed_Hz);
}

//! \brief     Set the SPDCALC controller
//! \param[in] handle   The SPDCALC controller handle
static inline void SPDCALC_run(SPDCALC_Handle handle, float32_t theta)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;

    obj->ref  = theta;

    // error cal
    obj->err = obj->ref - obj->fbk;

    // roll in the error
    if(obj->err >= MATH_PI)
    {
        obj->err = obj->err - MATH_TWO_PI;
    }
    else if(obj->err <= -MATH_PI)
    {
        obj->err = obj->err + MATH_TWO_PI;
    }

    // P and I control
    obj->Up  = obj->Kp * obj->err;              // P control
    obj->Ui += obj->Ki * obj->err;              // I control
    obj->Ui  = MATH_sat(obj->Ui, obj->Umax, obj->Umin);

    // control output
    obj->out = obj->Up + obj->Ui;
    obj->out = MATH_sat(obj->out, obj->Umax, obj->Umin);      // rad/s

    obj->speed_Hz = obj->out * MATH_ONE_OVER_TWO_PI;

    // Latest angle feedback estimation --> ( Fbk = integral of speed )
    obj->fbk = obj->fbk + obj->out * obj->thetaDelta;

    // roll "Fbk" within -pi to pi
    if(obj->fbk >= MATH_PI)
    {
        obj->fbk = obj->fbk - MATH_TWO_PI;
    }
    else if(obj->fbk <= -MATH_PI)
    {
        obj->fbk = obj->fbk + MATH_TWO_PI;
    }

    return;
}


extern void speedcal_pll_init(void);
extern SPDCALC_Obj speedpll[2];


//*****************************************************************************
//
// Close the Doxygen group.
//! @}
//
//*****************************************************************************

//*****************************************************************************
//
// Mark the end of the C bindings section for C++ compilers.
//
//*****************************************************************************
#ifdef __cplusplus
}
#endif

#endif //end of SPEED_CALC_H definition
